#ifndef MY_PHYSICS_H
#define MY_PHYSICS_H

#include "cg/cg.h"

namespace PeanutQuest {

	class MyPhysics {
	private:

		float _minPeanutHeight;
		//JUMP stuff
		float _currentJumpingHeight;
		float _maxJumpingHeight;
		float _jumpingIncrement;
		float _fallingIncrement;
		bool _isJumping;
		bool _ascending;
		// WALKING stuff
		float _currentWalkingHeight;
		float _maxWalkingHeight;
		float _walkingIncrement;
		float _walkingDecrement;

		cg::Vector3d _position;
		cg::Vector3d _front, _up, _right, _frontPoint;

		double _linearVelocity;
		double _angularVelocity;

		cg::Quaterniond _orientation;
		cg::Quaterniond _q;
		double _rotationMatrix[16];
		
		bool _isToJump;
		bool _isGoAhead, _isGoBack;
		bool _isTurnLeft, _isTurnRight;
		void rotate(double elapsed_seconds,int direction, cg::Vector3d axis, cg::Vector3d& v1, cg::Vector3d& v2);

		double _axesScale;

	public:

		MyPhysics();
		~MyPhysics();
		
		cg::Vector3d getPosition();
		cg::Quaterniond getOrientation();
		cg::Vector3d getFront();
		cg::Vector3d getUp();
		cg::Vector3d getRight();

		void goAhead();
		void goBack();
		void turnLeft();
		void turnRight();
		void jump();
		float getYpos();

		void setPosition(double x, double y, double z);
		void setLinearVelocity(double value);
		void setAngularVelocity(double value);

		void step(double elapsed_seconds);
		void applyTransforms();

		//TODO
		void shoesPosition();

		void setAxesScale(double scale);
		void drawAxes();
	};

}

#endif